#include <apf.h>

vector<double> APFPlanner::computeAngle()
{
    vector<double> Y;
    double deltax,deltay,r;
    for(int i=0;i<obs_num+1;i++)
    {
        if(i!=0)
        {
            deltax=this->obstacles[i-1][0]-start_point[0];
            deltay=this->obstacles[i-1][1]-start_point[1];
        }
        else
        {
            deltax=this->goal_point[0]-start_point[0];
            deltay=this->goal_point[1]-start_point[1];
        }
        r=sqrt(deltax*deltax+deltay*deltay);
        if(deltay>0)
            Y.push_back(acos(deltax/r));
        else
            Y.push_back(-acos(deltax/r));
    }
    return Y;
}
vector<double> APFPlanner::computeAttraction(vector<double> att_angle)
{
    double R=(goal_point[0]-start_point[0])*(goal_point[0]-start_point[0])\
        +(goal_point[1]-start_point[1])*(goal_point[1]-start_point[1]);
    double r=sqrt(R);
    vector<double> Yatt;
    Yatt.resize(2);
    Yatt[0]=Attraction_K*r*cos(att_angle[0]);
    Yatt[1]=Attraction_K*r*sin(att_angle[0]);
    return Yatt;
}
vector<double> APFPlanner::computeRepulsion(vector<double> angle)
{
    vector<double> YY;
    double Rat=(start_point[0]-goal_point[0])*(start_point[0]-goal_point[0])\
                    +(start_point[1]-goal_point[1])*(start_point[1]-goal_point[1]);
    double rat=sqrt(Rat);
    double Rre,rre,Yrer,Yata;
    vector<double> Yrerx, Yrery, Yatax, Yatay;
    Yrerx.resize(obs_num);
    Yrery.resize(obs_num);
    Yatax.resize(obs_num);
    Yatay.resize(obs_num);
    for(int i=0;i<obs_num;i++)
    {
        Rre=(start_point[0]-obstacles[i][0])*(start_point[0]-obstacles[i][0])\
                +(start_point[1]-obstacles[i][1])*(start_point[1]-obstacles[i][1]);
        rre=sqrt(Rre);
        if(rre>Obstacles_dis)
        {
            Yrerx[i]=0;
            Yrery[i]=0;
            Yatax[i]=0;
            Yatay[i]=0;
        }
        else if(rre>Obstacles_dis/2)
        {
            Yrer=Repulsion_K*(1/rre-1/Obstacles_dis)*(1/Rre)*Rat;//分解的Fre1向量
            Yata=Repulsion_K*((1/rre-1/Obstacles_dis)*(1/rre-1/Obstacles_dis))*rat;//分解的Fre2向量       Yata(i)=0;
            Yrerx[i]=Yrer*cos(angle[i+1]+3.1415 );  //angle_re(i)=Y(i+1)
            Yrery[i]=Yrer*sin(angle[i+1]+3.1415 );
            Yatax[i]=Yata*cos(angle[0]);            //angle_at=Y(1)
            Yatay[i]=Yata*sin(angle[0]);
        }
        else if(rre<Obstacles_dis/2)
        {
            Yrer=Repulsion_K*(1/rre-1/Obstacles_dis)*(1/Rre)*(pow(rat,a));//分解的Fre1向量
            Yata=a*Repulsion_K*((1/rre-1/Obstacles_dis)*(1/rre-1/Obstacles_dis))*(pow(rat,(1-a)))/2;//分解的Fre2向量   Yata(i)=0;
            Yrerx[i]=Yrer*cos(angle[i+1]+3.1415);  //angle_re(i)=Y(i+1)
            Yrery[i]=Yrer*sin(angle[i+1]+3.1415);
            Yatax[i]=Yata*cos(angle[0]);            //angle_at=Y(1)
            Yatay[i]=Yata*sin(angle[0]);
        }
    }
    YY.push_back(sum(Yrerx));
    YY.push_back(sum(Yrery));
    YY.push_back(sum(Yatax));
    YY.push_back(sum(Yatay));
    return YY;
}
double APFPlanner::sum(vector<double> p)
{
    double sum=0;
    for(int i=0;i<obs_num;i++)
    {
        sum+=p[i];
    }
    return sum;
}

double APFPlanner::get_apf_angle()
{
    vector<double> angle_re = computeAngle();
    vector<double> Yatt = computeAttraction(angle_re);
    vector<double> Y = computeRepulsion(angle_re);

    double Fsumyj=Yatt[1]+Y[1]+Y[3];
    double Fsumxj=Yatt[0]+Y[0]+Y[2];
    double Position_angle=atan2(Fsumyj,Fsumxj);
    return Position_angle;
}
void APFPlanner::set_obstacle(vector<vector <double> > obs)
{
    obstacles= obs;
    obs_num = obstacles.size();
    dis_to_goal = sqrt(pow(goal_point[0]-start_point[0],2)+\
        pow(goal_point[1]-start_point[1],2));
}
